

#include "cybertron/math/CartesianCoordinateSystem.hpp"
#include "cybertron/glm/dmat4.hpp"

CYBERTRON_BEGIN

CybertronCoreAPI CartesianCoordinateSystem::CartesianCoordinateSystem()
{
	mCenter = dvec3(0, 0, 0);
	mAxisX = dvec3(1, 0, 0);
	mAxisY = dvec3(0, 1, 0);
	mAxisZ = dvec3(0, 0, 1);
}

CybertronCoreAPI CartesianCoordinateSystem::CartesianCoordinateSystem(const CartesianCoordinateSystem& rhs)
{
	mCenter = rhs.mCenter;
	mAxisX = rhs.mAxisX;
	mAxisY = rhs.mAxisY;
	mAxisZ = rhs.mAxisZ;
}

CybertronCoreAPI CartesianCoordinateSystem& CartesianCoordinateSystem::operator = (const CartesianCoordinateSystem& rhs)
{
	mCenter = rhs.mCenter;
	mAxisX = rhs.mAxisX;
	mAxisY = rhs.mAxisY;
	mAxisZ = rhs.mAxisZ;

	return *this;
}

CybertronCoreAPI CartesianCoordinateSystem::~CartesianCoordinateSystem()
{
	// do nothing.
}

CybertronCoreAPI void CartesianCoordinateSystem::clear()
{
	mCenter = dvec3(0, 0, 0);
	mAxisX = dvec3(1, 0, 0);
	mAxisY = dvec3(0, 1, 0);
	mAxisZ = dvec3(0, 0, 1);
}

CybertronCoreAPI void CartesianCoordinateSystem::rotateX(double pitch)
{
	dmat4 matRotate = rotate(pitch, mAxisX);

	mAxisY = matRotate * mAxisY;
	mAxisZ = matRotate * mAxisZ;
}

CybertronCoreAPI void CartesianCoordinateSystem::rotateY(double yaw)
{
	dmat4 matRotate = rotate(yaw, mAxisY);

	mAxisX = matRotate * mAxisX;
	mAxisZ = matRotate * mAxisZ;
}

CybertronCoreAPI void CartesianCoordinateSystem::rotateZ(double rollRadian)
{
	dmat4 matRotate = rotate(rollRadian, mAxisZ);

	mAxisX = matRotate * mAxisX;
	mAxisY = matRotate * mAxisY;
}

CybertronCoreAPI void CartesianCoordinateSystem::normalize()
{
	mAxisX = cross(mAxisY, mAxisZ);
	mAxisX = cybertron::normalize(mAxisX);
	mAxisY = cross(mAxisZ, mAxisX);
	mAxisY = cybertron::normalize(mAxisY);
}

CybertronCoreAPI dvec3 CartesianCoordinateSystem::localToWorld(const dvec3& local) const
{
	return mCenter + mAxisX * local.x + mAxisY * local.y + mAxisZ * local.z;
}

CybertronCoreAPI dvec3 CartesianCoordinateSystem::worldToLocal(const dvec3& world) const
{
	dvec3 p = world - mCenter;

	return dvec3(
		dot(p, mAxisX),
		dot(p, mAxisY),
		dot(p, mAxisZ));
}

CybertronCoreAPI dvec3 CartesianCoordinateSystem::localToWorldDir(const dvec3& local) const
{
	return mAxisX * local.x + mAxisY * local.y + mAxisZ * local.z;
}

CybertronCoreAPI dvec3 CartesianCoordinateSystem::worldToLocalDir(const dvec3& p) const
{
	return dvec3(
		dot(p, mAxisX),
		dot(p, mAxisY),
		dot(p, mAxisZ));
}

CybertronCoreAPI const dvec3& CartesianCoordinateSystem::getCenter() const
{
	return mCenter;
}

CybertronCoreAPI void CartesianCoordinateSystem::setCenter(const dvec3& center)
{
	mCenter = center;
}

CybertronCoreAPI const dvec3& CartesianCoordinateSystem::getAxisX() const
{
	return mAxisX;
}

CybertronCoreAPI void CartesianCoordinateSystem::setAxisX(const dvec3& axisX)
{
	mAxisX = axisX;
}

CybertronCoreAPI const dvec3& CartesianCoordinateSystem::getAxisY() const
{
	return mAxisY;
}

CybertronCoreAPI void CartesianCoordinateSystem::setAxisY(const dvec3& axisY)
{
	mAxisY = axisY;
}

CybertronCoreAPI const dvec3& CartesianCoordinateSystem::getAxisZ() const
{
	return mAxisZ;
}

CybertronCoreAPI void CartesianCoordinateSystem::setAxisZ(const dvec3& axisZ)
{
	mAxisZ = axisZ;
}

CYBERTRON_END
